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filter designed by this method is observed as a function of measure- 
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APPLICATION OF THE KALMAN FILTER TO SPHERICAL 
DISCRETE TRACK SMOOTHING AND PREDICTION 

1,0 Introduction 

The Kalman filter is capable of estimating the state variables 
of a linear dynamic system under conditions of random perturbation of 
that system and noisy observation of a linear combination or combina- 
tions of the state variables. The motion of an airborne target, though 
presumably describable by linear dynamic equations in a cartesian frame, 
is most often observed in a spherical coordinate frame, as is the case 
in radar tracking. The transformation from cartesian to spherical co- 
ordinates is of course a nonlinear transformation, and does not allow 
the formulation of a constant observability matrix as required to the 
implementation of the Kalman estimation scheme. 

This paper investigates the effectiveness of a method for cir- 
cumventing the problem of nonlinear observation of a linear system 1 s 
state variables. The method employs a linearization of the cartesian- 
to-spherical coordinate transformation. The performance of a filter 
designed by this method is observed as a function of measurement 
error, accuracy of filter initialization, and the relevance of the 
estimated initial error covariance, P 2 | 

The investigation reveals that the most critical factor in fil- 
ter performance and stability is filter state initialization. The 
filter performs remarkably well, even in the face of considerable mea- 
surement error, provided it gets a "good start", i.e., a fairly ac- 
curate initialization of the filter states. The linearization scheme 
yields instability, however, for poor initialization. This is rather 
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unfortunate since in a realistic situation the accuracy of initiali- 
zation is a direct function of measurement error. The results under- 
score the need for more powerful initialization schemes for use in 
this and related nonlinear filtering problems. 

Included are digital computer simulations of filter performance. 
2.0 Linearization of the Coordinate Transformation 

Figure 1 represents in block diagram form the discrete state tran- 
sition equations for the dynamic process and the Kalman filter. All 
operaiians .within the dotted contour are carried out in cartesian co- 
ordinates. The observability transformation operator T is the non- 
linear cartesian-to-spherical coordinate transformation. The filter 
gain matrix G assumes and in fact requires for its calculation a 
linear observability matrix H. Such a matrix is obviously absent in 
the present problem. 

The linearization proceeds from the observation that the quan- 



tity 




(i) 



is the difference between two hopefully similar quantities, and may 
thus be classified as a "small difference". Dropping the subscripts 
from equation (1) for convenience and re-arranging, 

x = X - x (2) 

We now note that 

£ = T[£]=T[x-x] (3) 

A Taylor series expansion of equation (3) about x yields 

z = T [ x 1 - T- x + (4) 

— — ox — 
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Figure 1. Block diagram of the dynamic process and the 
Kalman filter estimating the State variables 
of that process. 



Spherical 




where the higher-order derivatives have been dropped. We may also 
write 



= z 



$ 



Noting that 

_z=T[x]+v 

equation (5) combined with (4) will give 



~ r 

z = T L x 





( 5 ) 

( 6 ) 



or 



~ 3t | ~ 

z = v -\ x 



( 7 ) 



The role of the partial derivative in equation (7) is seen by com- 
parison with the corresponding equation for linear observability, 
namely 

~ A 

z=Hx+v-Hx 



or z = v 4- H(x - x) = y + H x (8) 

The partial of T with respect to x> evaluated at x rather than x be- 
cause the latter is not available, may now be used to fill the role 
of H in the calculation of the filter gain matrix G for each sample. 

3.0 Filter Simulation 

3.1 Target Track and Dynamics 

The true target information that follows is common to all subse- 
quent examples. It assumes the tracking radar to be carried by an 
airborne interceptor. 
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Initial target position 



North 60 miles 



East 10 miles 



Down 2 miles 



Initial target heading 180° 



Interceptor heading 



000° throughout 



Initial target velocity 0.1473 miles per second 



Interceptor velocity 



0.148 miles per second, throughout 



The target executes the following maneuver: 



at t = 106 seconds, acceleration as follows 

p 

North +0.001715 miles per second 
East -0.00474 miles per second^ 



This acceleration continues for twenty seconds, at which time the tar- 
get is headed 220° with a velocity of 0.1473 miles per second. The 
duration of the run is 180 seconds. 

Observations are taken every two seconds. The target dynamics 
in each of the three cartesian coordinates have been represented by 



No attempt was made either to estimate or to sense target maneu- 
vers. The filter was kept "open" to the possibility of maneuvers by 
assigning, for purposes of filter gain calculation, an arbitrarily 



effectively prohibits the convergence of filter gains to zero, and 
hence keeps active the link between the target and the filter. 




small variance to the random disturbance w^. The resulting non-zero 
Q matrix, where 




(9) 
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At each observation, it was necessary to solve, in addition to 



the conversion, dynamic, smoothing and prediction equations implied by 
Figure 1, each of the following equations for determination of the fil- 
ter gains: 

G k = P k | k -l < \ | k _! < + R ] 1 < 10 > 



P k +1 I k 0 t P k I k -l 



G 1c M k F 



k k 






0 T + Q 



(ll) 



P j is the covariance matrix of prediction error. The M matrix is 

k | k-l k 

found as 



M. = | 

k 9x A A 



( 12 ) 



- = I k -l 



and must be evaluated at every observation. R is simply the covariance 
matrix of spherical observation noise. 

3.2 Example 1 

In example 1, the radar observables are range, range rate, and the 
two angles 0 and 0, as depicted in Figure 2. The standard deviations for 
spherical observation noise are as follows: 



cr 

range 


= 1.437 miles 


CT e 


= 0.0035 radians 


CT 0 


= 0.0174 radians 


cr. 


= 0.00152 miles/second 



Linearization of the coordinate transformation 



r = Vn 2 + 


E 2 4- D 2 


(13) 


0 = tan ^ 


E 

N 


(14) 


eh = D 




(15) 


M + 
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Figure 2. Spherical coordinates for 
Example 1. 
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NN + EE + DD 

; (i6) 



yielded 



N 

r 



n 2 + 5 2 



0 0 



0 0 



M = 



-ND 



2 / 
r V N 



0 0 



2 2 
N + E z 



E 

r 



2 2 
N + B 



0 0 



0 0 



-DE 



2 / 
r VN 



2 2 
N + E 



2- 



~ i 

0 o 



0 0 



o v4 2 + 



E 0 o 



r N-Ncy n o r E-EQ? 0 r D-D^ D 0 



(17) 



where ot = NN + EE + DD. The filter cartesian state variables were in- 
itialized by inversion of the coordinate transformation through velocity, 
using first differences from the initial two observations where necessary. 
Although, as will be shown, the initialization and bootstrapping employed 
in this example failed in some cases to yield a stable track, it is now 
described in some detail as an illustration of the difficulties en- 
countered in initialization. The observations are numbered starting 
with 1. The following expressions describe the starting procedure. The 
variables are as shown in Figure 1. 

a) Observation 1 yields z 

“1 

b) Observation 2 yields z 

~2 

c) Inverse coordinate transformation and first differences on ob- 
servations 1 and 2 yield position and velocity in all three 
cartesian coordinates. Accelerations are arbitrarily set to 
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are now set to these 



zero. The filter cartesian states x 

~2 I 2 

values . 

d) x . is calculated 

-3 1 2 

e) z , is calculated 

-3 1 2 

3t 1 

f) M = — * is calculated 

2 5x x = 

g) G and P are calculated as in equations (10) and (11), 

2 3 1 2 

using P , the initializing value of prediction error co- 

2 1 1 

variance, and M . It would appear that a further smoothing 
2 

of filter states would now be possible, since a filter gain 

matrix G has been calculated. We note, however, that there 
2 

ft A A 

has not been generated an x upon which x and z . de- 

-ih -2 1 1 -2 1 1 

pend. Without the latter we cannot generate 'z = z - z 

“2 “2 -2 I 1 

It is for this reason that the first smoothing is deferred to 
the third observation. 



3 | 2 

i) G and P . are calculated from P . and M 

3 A | 3 3 | 2 3 

j) Observation 3 yields 

ft A A 

k) Smoothing occurs, x . = x + G (z - z , ) 

—3 | 3 -3 | 2 3-3 —3 | 2 y 

Bootstrapping is now complete, and the filter operates from this point 
on in a straight forward fashion. 

It is not implied and not to be inferred that this initialization and 



h) M = — | 

3 A 



X = X 
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bootstrapping scheme is more effective or more desirable than any one 
of many other such methods. Serious questions can, in fact, be raised 



about the fact that the initializing value P . is not the one used 

2 | 1 

in the first smoothing. The first filter gain used, G^, is based upon 
P3 | 2> a calculated value. It is for these reasons that subsequent 
examples will attempt, insofar as possible, to isolate the effects of 
obprvation noise, error covariance initialization, and filter state 
initialization . 

Returning now to example 1 , there remains only to note that P^ | ^ 
was chosen as 





1 

1 



( 18 ) 



The raw radar data and the smoothed track for the NE plane, both 
compared with the true track (as seen from the interceptor) are shown 
in Figure 3 . This run was chosen as example 1 because it typifies 
the transition from stable to unstable filter performance for the 
initialization scheme outlined above. The smoothed track shows 
evidence of indecision in the early stages, an indecision that be- 
comes instability for higher observation noise levels. The filter 
is observed to be quite effective for the better part of the track. 
Runs performed at lower noise yielded virtually immediate lock-on 
to a very satisfactory smoothed track. 
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3.3 Example 2 

Example 2 seeks to isolate the effect of measurement noise upon 
filter performance. The first step taken here was to bypass the boot- 
strapping procedure. This was accomplished by starting the program 
at step h) in the bootstrap outline given above, x^ | ^ was set equal 
to the true x^» while P^ | 2 9 c o var i ance of prediction error asso- 
ciated with x , , was properly set to zero. Hence the filter is 

—3 I 2 

getting not only an error-free start with: respect to the first set of 
predicted filter states, but a precise corresponding conditional co- 
variance initialization as well. This effectively isolates measure- 
ment noise as a variable in the problem. The initial value of zero 
for covariance acts to effectively shut off filter gains until the P 
matrix starts accumulating value from the nominally small value of Q. 
This process of growth for the P and G matrices is readily seen by 
inspection of equations (10) and (11). The low initial values of G 
allow the error-free initial filter states to proceed very nearly 
along the true track, virtually free of any correction arising from 
observation noise, until such time as P has grown to an appreciable 
level . 

Nine runs are presented in this example. In the first run, the 
measurement noise standard deviations are one-tenth of the corres- 
ponding values in example 1. In each of the eight subsequent runs, the 
standard deviations are increased by a factor of 2. Figure 4 shows the 
set of 9 smoothed tracks, shown with the true track, for the NE plane. 
The corresponding raw observed tracks are not included, but an apprecia- 
tion for these tracks might be obtained by noting that the raw track 
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True 



Raw 



Filtered 



Figure 3. True, raw, and filtered tracks, NE plane, for Example 1. 
Scale: 7 miles per inch. 
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of Figure 3, . example 1, would fit roughly between the raw tracks 
underlying smoothed tracks 4 and 5 of Figure 4. We may further note 
that the raw track for run 9, Figure 4, is roughly 23 times as noisy 
as that shown in Figure 3a. 

We can only conclude from the results of example 2 that an ever- 
increasing level of measurement noise is not of itself sufficient to 
render the filter unstable, at least in the case of error-free state 
and covariance initialization. This tends to indict filter state in- 
itialization and/or conditional covariance initialization as the weak 
points leading to instability. This is looked into in example 3. 

3. Example 3 

In example 3, observation noise levels are identical for each of 
seven runs and are equal to those given in example 1. The starting 
bootstrap was once again bypassed. In the first run the filter was 
given an error-free start, (x^ | £ = X 3 ) > and p 3 | 2 was accordingly set 
equal to zero. In the subsequent six runs, increasingly larger state 
initialization errors were introduced, while concurrently P^ | 2 was 
incremented to accurately reflect this increase. In run 2, P^ | ^ was 
set to four-hundredths of the value shown in equation (18) and was in 
each subsequent run increased by a factor of four. In runs 2 through 
7, the random number generation subroutine, serving as a source of 
error for both initialization and measurement noise, was re-initialized 
so as to produce for each run the same base state initialization error 
vector and the same sequence of measurement errors. Hence all state 
initializations lie on the same vector, differing only in magnitude. 
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2 

Figure 4 



3 4 5 



6 7 8 



True and filtered tracks, NE plane, for Example 2. 
Scaler 7 miles per inch 



14 



Figure 5 shows the smoothed track in the NE plane for all seven 
runs of example 3. The raw track for each run is identical to that of 
Figure 3. The true track is, of course, also identical to that of 
Figure 3. 

We may conclude from the results of example 3 that accurate co- 
variance initialization does not of itself remove the possibility of 
filter instability. 

State initialization, then, appears to be especially critical. 

3.5 Example 4 

One final example was chosen to investigate the possibility that 
the fixed direction of the initial state error vector of example 3 
might have been a misleading factor in arriving at the conclusions 
drawn. Nine runs were made in example 4, with all conditions identi- 
cal to those of run 4 in example 3 except for the fact that the ran- 
dom number subroutine was not re-initialized for each run. This pro- 
duced the ensemble of smoothed tracks for the NE plane, shown in Fig- 
ure 6. These tracks have been grouped to show those considered good, 
poor, and unstable. 

We may conclude from example 4 that although the covariance 
initialization may accurately reflect the statistical properties of 
initial state errors, individual samples of the latter may be quite 
far removed from the mean, adding another degree of sensitivity to 
the initialization problem. No effort was made to establish a cor- 
relation between the tracks of Figure 6 and the extent of dispersion 
in the nine different initial state error vectors drawn from the same 
Gaussian distribution. 



15 



1 



2 



3 



4 



Figure' 5. Filtered tracks, NE plane, 
for Example 3. 

Scale: 7 miles per inch 
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Figure 6. Filtered tracks, NE plane, for Example 4. 
Scale: 7 miles per inch. 
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4.0 Conclusion 

Filter state initialization appears to be the most critical fac- 
tor in filter performance and stability. A bootstrap starting pro- 
cedure, to be effective, must place the initial state vector within 
certain bounds. Although no algorithm is presented for analytically 
determining these bounds, their existence is evident in Figure 5 , 
where filter performance is seen to deteriorate starting with run four. 
The bounds are very likely strongly dependent upon measurement noise 
levels . 

An effective bootstrap procedure must also include the means of 
determining meaningfully all elements of the initial error covariance 
matrix. This is not a simple task, mainly because nonlinear observ- 
ability leads to dependence between initial state estimates, and hence 
a full initial P matrix. 
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